from flask import Flask
from flask_cors import CORS
import os
import math
from datetime import datetime
import numpy as np


app = Flask(__name__)
CORS(app)

@app.route("/slam/data")
def slam_data():
    directory_path = os.path.join(os.getcwd(), "public", "slam", "data")
    url_path = "/slam/data/"
    all_items = os.listdir(directory_path)
    payload = []
    for item in all_items:
        payload.append({
            "name": item.split(".")[0],
            "video": url_path + item
        })
    return payload

@app.route("/slam/results")
def slam_results():
    directory_path = os.path.join(os.getcwd(), "public", "slam", "results")
    url_path = "/slam/results/"
    all_items = os.listdir(directory_path)
    payload = []
    for item in all_items:
        payload.append({
            "name": item,
            "video": url_path + item + "/定位视频.mp4",
            "xyz": url_path + item + "/xyz图.png",
            "ape": url_path + item + "/APE折线图.png",
            "apeValue": url_path + item + "/APE值.png",
            "rpe": url_path + item + "/RPE折线图.png",
            "rpeValue": url_path + item + "/RPE值.png",
        })
    return payload

@app.route("/traj/data")
def traj_data():
    directory_path = os.path.join(os.getcwd(), "public", "traj", "data")
    url_path = "/traj/data/"
    all_items = os.listdir(directory_path)
    payload = []
    for item in all_items:
        payload.append({
            "name": item,
            "video": url_path + item + "/规划视频.mp4",
            "photo": url_path + item + "/规划场景方向.png",
        })
    return payload

@app.route("/traj/results")
def traj_results():
    directory_path = os.path.join(os.getcwd(), "public", "traj", "results")
    all_items = os.listdir(directory_path)
    payload = []
    for item in all_items:
        payload.append({
            "name": item,
        })
    return payload

@app.route("/traj/results/<name>")
def result_info(name):
    txt_path = os.path.join(os.getcwd(), "public", "traj", "results", name)
    
    x = []
    y = []
    z = []
    points = []
    length = 0

    with open(txt_path, 'r', encoding='utf-8') as file:
        lines = file.readlines()
        dt1 = datetime.fromtimestamp(float(lines[1].split(",")[0]) / 1e9)
        dt2 = datetime.fromtimestamp(int(lines[-1].split(",")[0])/ 1e9)
        diff = dt2 - dt1
        pre = list(map(float, lines[1].split(",")[5:8]))
        for line in lines[1:]:
            point = list(map(float, line.split(",")[5:8]))
            points.append(point)
            x.append(point[0])
            y.append(point[1])
            z.append(point[2])
            length += distance_3d(pre, point)
            pre = point
        # 计算曲线的曲率
        curvatures = curvature_3d(x, y, z)
        # 计算曲率的统计指标
        mean_curvature = np.mean(curvatures)
        variance_curvature = np.var(curvatures)
        max_curvature = np.max(curvatures)
        min_curvature = np.min(curvatures)

        print("平均曲率：", mean_curvature)
        print("曲率方差：", variance_curvature)
        print("最大曲率：", max_curvature)
        print("最小曲率：", min_curvature)

    return {
        "name": name,
        "points": points,
        "len": length,
        "duration": diff.total_seconds(),
        "curvature": curvatures,
        "meanCurvature": mean_curvature,
        "varianceCurvature": variance_curvature
    }

def distance_3d(point1, point2):
    x1, y1, z1 = point1
    x2, y2, z2 = point2
    return math.sqrt((x2 - x1) ** 2 + (y2 - y1) ** 2 + (z2 - z1) ** 2)

def curvature_3d(x_list, y_list, z_list):
    """
    计算3D曲线的曲率，输入为x坐标列表、y坐标列表和z坐标列表。

    参数:
    x_list (list): x坐标列表，对应曲线上各点的x坐标。
    y_list (list): y坐标列表，对应曲线上各点的y坐标。
    z_list (list): z坐标列表，对应曲线上各点的z坐标。

    返回:
    curvatures (list): 曲线上各点的曲率列表。
    """
    num_points = len(x_list)
    curvatures = []
    h = 1e-6  # 用于数值差分的微小步长

    for i in range(1, num_points - 1):
        # 数值差分近似计算一阶导数
        dx_dt = (x_list[i + 1] - x_list[i - 1]) / (2 * h)
        dy_dt = (y_list[i + 1] - y_list[i - 1]) / (2 * h)
        dz_dt = (z_list[i + 1] - z_list[i - 1]) / (2 * h)

        # 数值差分近似计算二阶导数
        ddx_dt2 = (x_list[i + 1] - 2 * x_list[i] + x_list[i - 1]) / h ** 2
        ddy_dt2 = (y_list[i + 1] - 2 * y_list[i] + y_list[i - 1]) / h ** 2
        ddz_dt2 = (z_list[i + 1] - 2 * z_list[i] + z_list[i - 1]) / h ** 2

        r_prime = np.array([dx_dt, dy_dt, dz_dt])
        r_prime_prime = np.array([ddx_dt2, ddy_dt2, ddz_dt2])

        cross_product = np.cross(r_prime, r_prime_prime)
        numerator = np.linalg.norm(cross_product)
        denominator = np.linalg.norm(r_prime) ** 3

        curvature = numerator / denominator
        curvatures.append(curvature)

    return curvatures